package com.rockwell.robot2015.model.states.running.sequence;

import com.rockwell.robot2015.model.MazeType;
import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.solvers.IMazeSolver;
import com.rockwell.robot2015.model.solvers.LeftFirstMazeSolver;
import com.rockwell.robot2015.model.solvers.RightFirstMazeSolver;
import com.rockwell.robot2015.model.solvers.SuperMazeSolver;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public class ExitingMazeSequence extends RobotRunningSequence {
	static MazeType m_mazeType = MazeType.None;
	
	static IMazeSolver[] m_solvers = new IMazeSolver[] {
		new LeftFirstMazeSolver(),
		new RightFirstMazeSolver()
	};
	
	static IMazeSolver m_solver;
	static boolean m_justStarted;
	
	enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().log("Start exiting maze", 2);

				Robot robot = machine.robot();
				
				//m_mazeType = checkMaze(machine);
				
				/*
				if (m_mazeType == 3) {
					m_solver = new LoopMazeSolver(true);
				}
				else if (m_mazeType > 0){
					m_solver = m_solvers[m_mazeType-1];
				}
				else {
					m_solver = new LoopMazeSolver(true);
				}
				*/
				
				if (machine.robot().getCurrentMaze() == MazeType.Maze3) {
					m_solver = new SuperMazeSolver(true);
				}
				else {
					m_solver = new LeftFirstMazeSolver();
				}
				
				m_solver.reset(robot);
				
				m_justStarted = true;
				
				IRobotSensors sensor = robot.getSensor();
				
				robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
				
				if (sensor.detectFrontWall()) {
					machine.changeState(FrontWallDetected);
				}
				else if (sensor.detectLeftWall() && sensor.detectRightWall()) {
					machine.changeState(JogInWall);
				}
				else {
					IDetector detector = robot.getMazeEntranceGuideDetector();
					if (detector.isPresent()) {
						machine.changeState(ExitFound);
					}
					else {
						robot.log("Weired! No walls found, no exit found", 2);
						machine.changeState(JogInWall);
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		JogInWall {

			@Override
			public void enter(IStateMachine machine) {
				Robot robot = machine.robot();

				robot.log("Jog in walls", 4);
				
				if (m_justStarted || !robot.isJogging3()) {
					m_justStarted = false;
					machine.robot().startJogging3();
				}
				
				//m_history.add(RobotAction.Jog);
			}

			@Override
			public void move(IStateMachine machine) {
				// machine.robot().sendCommand(RobotAction.Jog);
				// m_history.add(RobotAction.Jog);

				Robot robot = machine.robot();

				IDetector detector = robot.getMazeEntranceGuideDetector();
				if (detector.isPresent()) {
					machine.changeState(ExitFound);
					return;
				}

				IRobotSensors sensor = robot.getSensor();

				if (robot.isJogDone()) {
					if (sensor.detectLeftWall() == false || sensor.detectRightWall() == false) {
						machine.changeState(JunctionReached);
					} else if (sensor.detectFrontWall() == true) {
						machine.changeState(FrontWallDetected);
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		OutOfJunction {

			@Override
			public void enter(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.log("Out of junction", 4);
				
				//robot.startJogging2();
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.sendCommand(RobotAction.Jog2);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
		
				robot.sendCommand(RobotAction.MoveWithoutSensor, IMazeSolver.AFTER_JUNCTION);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				IDetector detector = robot.getMazeEntranceGuideDetector();
				
				if (detector.isPresent()) {
					machine.changeState(ExitFound);
				}
				else {
					IRobotSensors sensor = robot.getSensor();

					if (!sensor.detectLeftWall() && !sensor.detectRightWall()) {
						machine.changeState(Exited);
					}
					else {
						machine.changeState(JogInWall);
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		JunctionReached {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Junction reached", 4);
				
				m_solver.onJunctionReached(machine.robot());
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.sendCommand(RobotAction.MoveWithoutSensor, IMazeSolver.JUNCTION_CLEARANCE);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				IRobotSensors sensor = robot.getSensor();
				if (!sensor.detectLeftWall() && !sensor.detectRightWall()) {
					machine.changeState(Exited);
				}
				else {
					m_solver.move(robot);
				
					machine.changeState(OutOfJunction);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}
		},
		FrontWallDetected {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Front wall detected", 4);
				
				m_solver.onFrontWallReached(machine.robot());
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.sendCommand(RobotAction.MoveWithoutSensor, IMazeSolver.JUNCTION_CLEARANCE);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				m_solver.move(robot);
				
				machine.changeState(OutOfJunction);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		ExitFound {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Exit found", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				if (robot.isJogDone()) {
					IRobotSensors sensor = robot.getSensor();
					
					if (!sensor.detectFrontWall()) {
						robot.sendCommand(RobotAction.MoveWithoutSensor, IMazeSolver.JUNCTION_CLEARANCE);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
						
						robot.sendCommand(RobotAction.Jog3);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
						
						machine.changeState(Exited);
					}
				}
				else {
					/*
					IDetector detector = robot.getMazeEntranceGuideDetector();
					if (detector.isPresent()) {
						machine.changeState(ExitFound);
					}
					*/
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Exited {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Exited maze successfully", 2);
				
				machine.robot().getSensor().setInMaze(false);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		};

		static ExitingMazeSequence m_sequence;

		public static void setSequence(ExitingMazeSequence sequence) {
			m_sequence = sequence;
		}
	}
	
	public ExitingMazeSequence(RobotRunningStateMachine running) {
		super(running, State.Start);

		State.setSequence(this);
		m_currState = State.Start;
	}

	@Override
	public void start() {
		m_currState = State.Start;
		m_nextState = null;
		
		Robot robot = m_running.robot();
		robot.enableTreasureDetector(false);
		//robot.enableObstacleDetector(false);
		robot.enableMaze1Detector(false);
		robot.enableMaze2Detector(false);
		robot.enableMaze3Detector(false);
		robot.enableMazeEntranceGuideDetector(true);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(false);
	}

	@Override
	public void reset() {
		super.reset();
	}

	@Override
	public boolean succeeded() {
		return m_currState.equals(State.Exited);
	}

	@Override
	public boolean failed() {
		// TODO Auto-generated method stub
		return false;
	}
	
	public void setMazeType(MazeType type) {
		m_mazeType = type;
	}
	
	public MazeType getMazeType() {
		return m_mazeType;
	}
}
